//! GJK algorithm

const std = @import("std");
const setting = @import("../setting.zig");
const Point = @import("../point.zig");
const Vec2 = @import("../vec2.zig");
const Transform = Point.Transform;

pub const Gjk = @This();

cache: SimplexCache,
allocator: std.mem.Allocator,

pub fn init(allocator: std.mem.Allocator) Gjk {
    return Gjk{
        .cache = SimplexCache.init(),
        .allocator = allocator,
    };
}

pub fn calc(self: *Gjk, _proxy_a: *const Proxy, tx_a: ?*const Transform, _proxy_b: *const Proxy, tx_b: ?*const Transform, use_radius: bool) GjkOutput {
    var proxy_a: *const Proxy = undefined;
    if (null == tx_a) {
        proxy_a = _proxy_a;
    } else {
        var local_proxy = self.allocator.create(Proxy) catch @panic("Out of memory");
        local_proxy.radius = _proxy_a.radius;
        for (0.._proxy_a.vertices.len) |i| {
            local_proxy.vertices[i] = _proxy_a.vertices[i].ifTransform(tx_a);
        }

        proxy_a = local_proxy;
    }
    defer self.allocator.free(proxy_a);

    var proxy_b: *const Proxy = undefined;
    if (null == tx_b) {
        proxy_b = _proxy_b;
    } else {
        var local_proxy = self.allocator.create(Proxy) catch @panic("Out of memory");
        local_proxy.radius = _proxy_b.radius;
        for (0.._proxy_b.vertices.len) |i| {
            local_proxy.vertices[i] = _proxy_b.vertices[i].ifTransform(tx_b);
        }

        proxy_b = local_proxy;
    }
    defer self.allocator.free(proxy_b);

    const simplex = self.makeSimplexFromCache(proxy_a, proxy_b);

    var non_unit_normal = Vec2.zero();

    // These store the vertices of the last simplex so that we can check for duplicates and prevent cycling.
    var save_a: [3]u16 = undefined;
    var save_b: [3]u16 = undefined;

    // Main iteration loop. All computations are done in frame A.
    const F32EpsilonSqr = std.math.floatEps(f32) * std.math.floatEps(f32);
    const MaxIterations: u8 = 20;
    var iteration: u8 = 0;
    while (iteration < MaxIterations) {
        const save_count = simplex.count;
        for (0..save_count) |i| {
            save_a[i] = simplex.v[i].index_a;
            save_b[i] = simplex.v[i].index_b;
        }

        var d: Vec2 = undefined;
        switch (simplex.count) {
            1 => {
                d = simplex.v[0].point_support.vectorTo(&Point.Origin);
            },
            2 => {
                d = simplex.solve2();
            },
            3 => {
                d = simplex.solve3();
            },
            else => {
                @panic("Unknow error for GJK algorithm.");
            },
        }

        if (simplex.count == 3) {
            break;
        }

        // Save the normal
        non_unit_normal = d;

        // Ensure the search direction is numerically fit.
        if (d.dot(&d) < F32EpsilonSqr) {
            // This is unlikely but could lead to bad cycling.
            // The branch predictor seems to make this check have low cost.

            // The origin is probably contained by a line segment
            // or triangle. Thus the shapes are overlapped.

            // We can't return zero here even though there may be overlap.
            // In case the simplex is a point, segment, or triangle it is difficult
            // to determine if the origin is contained in the CSO or very close to it.
            break;
        }

        // Compute a tentative new simplex vertex using support points.
        // support = support(a, d) - support(b, -d)
        var support_point_info = &simplex.v[simplex.count];
        support_point_info.index_a = findSupport(&proxy_a, &d);
        support_point_info.point_a = proxy_a.vertices[support_point_info.index_a];
        support_point_info.index_b = findSupport(&proxy_b, &d.ifNeg());
        support_point_info.point_b = proxy_b.vertices[support_point_info.index_b];

        // Iteration count is equated to the number of support point calls.
        iteration += 1;

        // Check for duplicate support points. This is the main termination criteria.
        var duplicate = false;
        for (0..save_count) |i| {
            if (support_point_info.index_a == save_a[i] and support_point_info.index_b == save_b[i]) {
                duplicate = true;
                break;
            }
        }

        // If we found a duplicate support point we must exit to avoid cycling.
        if (duplicate) {
            break;
        }

        // New vertex is valid and needed.
        simplex.count += 1;
    }

    // Prepare output
    var normal = non_unit_normal.normalize();
    var local_point_a: Point = undefined;
    var local_point_b: Point = undefined;
    computeSimplexWitnessPoint(&local_point_a, &local_point_b, &simplex);
    var gjk_output: GjkOutput = undefined;
    gjk_output.normal = normal;
    gjk_output.distance = @sqrt(local_point_a.distanceSquaredTo(local_point_b));
    gjk_output.point_a = local_point_a;
    gjk_output.point_b = local_point_b;

    makeSimplexFromCache(self, proxy_a, proxy_b);
    // Apply radii if requested
    if (use_radius and gjk_output.distance > 0.1 * setting.LinearSlop) {
        const radius_a = proxy_a.radius;
        const radius_b = proxy_b.radius;
        gjk_output.distance = @max(0.0, gjk_output.distance - radius_a - radius_b);

        // Keep closest points on perimeter even if overlapped, this way the points move smoothly.
        const vec_a = normal.ifMul(radius_a);
        gjk_output.point_a.moveAwayOrigin(vec_a.x, vec_a.y);
        const vec_b = normal.ifMul(radius_b);
        gjk_output.point_b.moveAwayOrigin(vec_b.x, vec_b.y);
    }

    return gjk_output;
}

fn computeSimplexWitnessPoint(a: *Point, b: *Point, s: *const Simplex) void {
    switch (s.count) {
        1 => {
            a.* = s.v[0].point_a;
            b.* = s.v[0].point_b;
        },
        2 => {
            a.* = weight2(s.v[0].bary_coord, &s.v[0].point_a, s.v[1].bary_coord, &s.v[1].point_a);
            b.* = weight2(s.v[0].bary_coord, &s.v[0].point_b, s.v[1].bary_coord, &s.v[1].point_b);
        },
        3 => {
            a.* = weight3(s.v[0].bary_coord, &s.v[0].point_a, s.v[1].bary_coord, &s.v[1].point_a, s.v[2].bary_coord, &s.v[2].point_a);
            b.* = weight3(s.v[0].bary_coord, &s.v[0].point_b, s.v[1].bary_coord, &s.v[1].point_b, s.v[2].bary_coord, &s.v[2].point_b);
        },
        else => {
            @panic("Unexpectd exception.");
        },
    }
}

fn weight2(a1: f32, w1: *const Point, a2: f32, w2: *const Point) Point {
    return Point.init(a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y);
}

fn weight3(a1: f32, w1: *const Point, a2: f32, w2: *const Point, a3: f32, w3: *const Point) Point {
    return Point.init(a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y);
}

fn findSupport(proxy: *const Proxy, direction: *const Vec2) usize {
    var best_index: usize = 0;
    var best_value: f32 = direction.dot(&Point.Origin.vectorTo(&proxy.vertices[0]));
    for (1..proxy.vertices.len) |i| {
        const value = direction.dot(&Point.Origin.vectorTo(&proxy.vertices[i]));
        if (value > best_value) {
            best_index = i;
            best_value = value;
        }
    }

    return best_index;
}

fn makeSimplexFromCache(self: *const Gjk, proxy_a: *const Proxy, proxy_b: *const Proxy) Simplex {
    var simplex = Simplex{
        .v = undefined,
        .count = self.cache.count,
    };

    for (0..simplex.count) |i| {
        var v = &simplex.v[i];

        v.index_a = self.cache.index_a[i];
        v.index_b = self.cache.index_b[i];
        v.point_a = proxy_a.vertices[v.index_a];
        v.point_b = proxy_b.vertices[v.index_b];
        v.point_support = v.point_b.ifMove2Origin(v.point_a.x, v.point_a.y);

        // invalid
        v.bary_coord = -1.0;
    }

    if (0 == simplex.count) {
        var v = &simplex.v[0];
        v.index_a = 0;
        v.index_b = 0;
        v.point_a = proxy_a.vertices[0];
        v.point_b = proxy_b.vertices[0];
        v.point_support = v.point_b.ifMove2Origin(v.point_a.x, v.point_a.y);
        v.bary_coord = 1.0;

        simplex.count = 1;
    }

    return simplex;
}

/// A distance proxy is used by the GJK algorithm. It encapsulates any shape.
pub const Proxy = struct {
    vertices: []const Point,
    radius: f32,

    pub fn init(vertices: []const Point, radius: f32) Proxy {
        return Proxy{
            .vertices = vertices,
            .radius = radius,
        };
    }
};

pub const GjkOutput = struct {
    /// Closest point on shapeA
    point_a: *Point,

    /// Closest point on shapeB
    point_b: *Point,

    /// Normal vector that points from A to B
    normal: Vec2,

    /// The final distance, zero if overlapped
    distance: f32,
};

pub const GjkInput = struct {
    /// The proxy for shape A
    proxy_a: *Proxy,

    /// The proxy for shape B
    proxy_b: *Proxy,

    /// The world transform for shape A
    tx_a: *Transform,

    /// The world transform for shape B
    tx_b: *Transform,

    /// Should the proxy radius be considered?
    use_radius: bool,
};

/// Support point from the GJK algorithm
pub const SupportPointInfo = struct {
    /// point from shape A
    point_a: *Point,

    /// point from shape B
    point_b: *Point,

    /// support point from point_a and point_b
    point_support: Point,

    /// barycentric coordinate for closest point
    bary_coord: f32,

    /// index of point_a
    index_a: u16,

    /// index of point_b
    index_b: u16,
};

/// Simplex from the GJK algorithm
pub const Simplex = struct {
    /// vertices
    v: [3]SupportPointInfo,

    /// number of valid vertices
    count: u16,

    /// Solve a line segment using barycentric coordinates.
    ///
    /// p = a1 * w1 + a2 * w2
    /// a1 + a2 = 1
    ///
    /// The vector from the origin to the closest point on the line is
    /// perpendicular to the line.
    /// e12 = w2 - w1
    /// dot(p, e) = 0
    /// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
    ///
    /// 2-by-2 linear system
    /// [1      1     ][a1] = [1]
    /// [w1.e12 w2.e12][a2] = [0]
    ///
    /// Define
    /// d12_1 =  dot(w2, e12)
    /// d12_2 = -dot(w1, e12)
    /// d12 = d12_1 + d12_2
    ///
    /// Solution
    /// a1 = d12_1 / d12
    /// a2 = d12_2 / d12
    ///
    /// returns a vector that points towards the origin
    pub fn solve2(self: *Simplex) Vec2 {
        const w1 = Point.Origin.vectorTo(&self.v[0].point_support);
        const w2 = Point.Origin.vectorTo(&self.v[1].point_support);
        const e12 = w2.ifSub(w1);

        const d12_2 = -w1.dot(e12);
        if (d12_2 <= 0.0) {
            self.v[0].bary_coord = 1.0;
            self.count = 1;

            return w1.ifNeg();
        }

        const d12_1 = -w2.dot(e12);
        if (d12_1 <= 0.0) {
            self.v[1].bary_coord = 1.0;
            self.count = 1;
            self.v[0] = self.v[1];

            return w2.ifNeg();
        }

        const inv_d12: f32 = 1.0 / (d12_1 + d12_2);
        self.v[0].bary_coord = d12_1 * inv_d12;
        self.v[1].bary_coord = d12_2 * inv_d12;
        self.count = 2;

        return e12.crossVS(w1.ifAdd(w2).cross(e12));
    }

    pub fn solve3(self: *Simplex) Vec2 {
        const w1 = Point.Origin.vectorTo(&self.v[0].point_support);
        const w2 = Point.Origin.vectorTo(&self.v[1].point_support);
        const w3 = Point.Origin.vectorTo(&self.v[2].point_support);

        // Edge12
        // [1      1     ][a1] = [1]
        // [w1.e12 w2.e12][a2] = [0]
        // a3 = 0
        const e12 = w2.ifSub(&w1);
        const w1e12 = w1.dot(&e12);
        const w2e12 = w2.dot(&e12);
        const d12_1 = w2e12;
        const d12_2 = -w1e12;

        // Edge13
        // [1      1     ][a1] = [1]
        // [w1.e13 w3.e13][a3] = [0]
        // a2 = 0
        const e13 = w3.ifSub(&w1);
        const w1e13 = w1.dot(&e13);
        const w3e13 = w3.dot(&e13);
        const d13_1 = w3e13;
        const d13_2 = -w1e13;

        // Edge23
        // [1      1     ][a2] = [1]
        // [w2.e23 w3.e23][a3] = [0]
        // a1 = 0
        const e23 = w3.ifSub(&w2);
        const w2e23 = w2.dot(&e23);
        const w3e23 = w3.dot(&e23);
        const d23_1 = w3e23;
        const d23_2 = -w2e23;

        // Triangle123
        const n123 = e12.cross(&e13);

        const d123_1 = n123 * w2.cross(w3);
        const d123_2 = n123 * w3.cross(w1);
        const d123_3 = n123 * w1.cross(w2);

        // w1 region
        if (d12_2 <= 0.0 and d13_2 <= 0.0) {
            self.v[0].bary_coord = 1.0;
            self.count = 1;
            return w1.ifNeg();
        }

        // e12
        if (d12_1 > 0.0 and d12_2 > 0.0 and d123_3 <= 0.0) {
            const inv_d12 = 1.0 / (d12_1 + d12_2);
            self.v[0].bary_coord = d12_1 * inv_d12;
            self.v[1].bary_coord = d12_2 * inv_d12;
            self.count = 2;

            return e12.crossVS(w1.ifAdd(&w2).cross(&e12));
        }

        // e13
        if (d13_1 > 0.0 and d13_2 > 0.0 and d123_2 <= 0.0) {
            const inv_d13 = 1.0 / (d13_1 + d13_2);
            self.v[0].bary_coord = d13_1 * inv_d13;
            self.v[2].bary_coord = d13_2 * inv_d13;
            self.count = 2;
            self.v[1] = self.v[2];

            return e13.crossVS(w1.ifAdd(&w3).cross(&e13));
        }

        // w2 region
        if (d12_1 <= 0.0 and d23_2 <= 0.0) {
            self.v[1].bary_coord = 1.0;
            self.count = 1;
            self.v[0] = self.v[1];

            return w2.ifNeg();
        }

        // w3 region
        if (d13_1 <= 0.0 and d23_1 <= 0.0) {
            self.v[2].bary_coord = 1.0;
            self.count = 1;
            self.v[0] = self.v[2];

            return w3.ifNeg();
        }

        // e23
        if (d23_1 > 0.0 and d23_2 > 0.0 and d123_1 <= 0.0) {
            const inv_d23 = 1.0 / (d23_1 + d23_2);
            self.v[1].bary_coord = d23_1 * inv_d23;
            self.v[2].bary_coord = d23_2 * inv_d23;
            self.count = 2;
            self.v[0] = self.v[2];

            return e23.crossVS(w2.ifAdd(&w3).cross(&e23));
        }

        // Must be in triangle123
        const inv_d123: f32 = 1.0 / (d123_1 + d123_2 + d123_3);
        self.v[0].bary_coord = d123_1 * inv_d123;
        self.v[1].bary_coord = d123_2 * inv_d123;
        self.v[2].bary_coord = d123_3 * inv_d123;
        self.count = 3;

        return Vec2.zero();
    }
};

/// Used to warm start the GJK simplex. If you call this function multiple times with nearby
/// transforms this might improve performance. Otherwise you can zero initialize this.
/// The distance cache must be initialized to zero on the first call.
/// Users should generally just zero initialize this structure for each call.
const SimplexCache = struct {
    /// The number of stored simplex points
    count: u16,

    /// The cached simplex indices on shape A
    index_a: [3]u16,

    /// The cached simplex indices on shape B
    index_b: [3]u16,

    pub fn init() SimplexCache {
        return SimplexCache{
            .count = 0,
            .index_a = [3]u16{0} ** 3,
            .index_b = [3]u16{0} ** 3,
        };
    }
};
